
#include <cpu.h>
#include <dev.h>
#include "access.h"
#include "pentium3.h"
#include "exception.h"

/* ------------------------------------------------------------------------- */

#define iopl					((eflags & FLAG_IOPL) >> 12)
#define desc_dpl(desc)			(int)(((desc) & 0x0000600000000000) >> 45)
#define desc_type(desc)			(int)(((desc) & 0x00000f0000000000) >> 40)
#define req_level(lv)			((cr0 & CR0_PE) && sel_cpl(cs) > (lv))
#define pgdir(la)				(((la) >> 22) & 0x000003ff)
#define pgtab(la)				(((la) >> 12) & 0x000003ff)
#define pgoff_4kb(la)			((la) & 0x00000fff)
#define pgoff_4mb(la)			((la) & 0x003fffff)

/* ------------------------------------------------------------------------- */

static inline uint32 seg_base(uint64 desc);
static inline uint32 seg_limit(uint64 desc);
static inline bool check_privilege_level(uint16 sel, uint64 desc);
static inline uint32 paging_4kb(uint32 la, uint32 pde);
static inline uint32 paging_4mb(uint32 la, uint32 pde);
static inline uint32 paging_2mb(uint32 la, uint32 pde);

static uint32 fastcall virtual_access(uint64 desc, uint32 va, size_t size);

/* ------------------------------------------------------------------------- */

void fastcall check_eflags_access(void)
{
	if ((cr0 & CR0_PE) && get_flag(FLAG_VM) && iopl != 3)
		raise_exception(EX_GP);
}

void fastcall check_cr_access(int cr_no)
{
	switch (cr_no) {
	case 0x00:	case 0x02:	case 0x03:	case 0x04:
		break;
	default:
		raise_exception(EX_UD);
	}

	if (req_level(0) || get_flag(FLAG_VM))
		raise_exception(EX_GP);
}

void fastcall check_dr_access(int dr_no)
{
	switch (dr_no) {
	case 0x00:	case 0x01:	case 0x02:	case 0x03:	case 0x06:	case 0x07:
		break;
	default:
		raise_exception(EX_UD);
	}

	if (req_level(0) || get_flag(FLAG_VM))
		raise_exception(EX_GP);

	if (dr7 & DR7_GD)
		raise_exception(EX_DB);
}

void fastcall check_msr_access(uint32 addr)
{
	if (req_level(0) || get_flag(FLAG_VM))
		raise_exception(EX_GP);
}

void fastcall check_lxdt_access(void)
{
	if (req_level(0))
		raise_exception(EX_GP);
}

void fastcall check_ltr_access(uint16 sel, uint64 desc)
{
	if (req_level(0))
		raise_exception(EX_GP);

	if (desc_type(desc) != 0x9)
		raise_exception_with_error(EX_GP, sel);

	if (!(desc & DESC_P))
		raise_exception_with_error(EX_NP, sel);
}

void fastcall check_port_access(void)
{
	if (!(cr0 & CR0_PE) || (sel_cpl(cs) <= iopl && !get_flag(FLAG_VM)))
		return;

	// TODO: TSS
}

void fastcall check_tsc_access(void)
{
	if ((cr4 & CR4_TSD) && req_level(0))
		raise_exception(EX_GP);
}

void fastcall check_if_access(void)
{
	if (!(cr0 & CR0_PE))
		return;

	if (get_flag(FLAG_VM)) {
		if (iopl == 3)
			return;
	} else {
		if (sel_cpl(cs) <= iopl)
			return;
	}

	raise_exception(EX_GP);
}

void fastcall check_cs_access(uint16 seg, uint64 xseg, uint16 sel, uint64 desc)
{
	/* null segment selector checking */
	if (sel < 4)
		raise_exception(EX_GP);

	if (sel_index(sel) >= (uint16)(sel_type(sel) ? ldtr : gdtr))
		raise_exception(EX_GP);

	if (xseg & DESC_C) {
		if (desc_dpl(xseg) > sel_cpl(cs))
			raise_exception_with_error(EX_GP, sel);
	} else {
		if (sel_rpl(sel) > sel_cpl(cs) || desc_dpl(xseg) != sel_cpl(cs))
			raise_exception_with_error(EX_GP, sel);
	}

	if (!(desc & DESC_P))
		raise_exception_with_error(EX_NP, sel);

	if (!(desc & DESC_S) || !(desc & DESC_T))
		raise_exception_with_error(EX_GP, sel);

	assert(sel_cpl(cs) == 0);
}

void fastcall check_ss_access(uint16 seg, uint64 xseg, uint16 sel, uint64 desc)
{
	/* null segment selector checking */
	if (sel < 4)
		raise_exception(EX_GP);

	if (sel_index(sel) >= (uint16)(sel_type(sel) ? ldtr : gdtr))
		raise_exception_with_error(EX_GP, sel);

	if (sel_rpl(sel) != sel_cpl(ss) || sel_rpl(sel) != desc_dpl(xseg))
		raise_exception_with_error(EX_GP, sel);

	if (!(desc & DESC_P))
		raise_exception_with_error(EX_SS, sel);

	if (!(desc & DESC_S) || (desc & DESC_T) || !(desc & DESC_W))
		raise_exception_with_error(EX_GP, sel);
}

void fastcall check_ds_access(uint16 seg, uint64 xseg, uint16 sel, uint64 desc)
{
	if (!sel_index(sel))
		return;

	if (sel_index(sel) >= (uint16)(sel_type(sel) ? ldtr : gdtr))
		raise_exception_with_error(EX_GP, sel);

	if (desc_dpl(xseg) < sel_cpl(cs) || desc_dpl(xseg) < sel_rpl(sel))
		raise_exception_with_error(EX_GP, sel);

	if (!(desc & DESC_P))
		raise_exception_with_error(EX_NP, sel);

	if (!(desc & DESC_S) || (desc & DESC_T) || !(desc & DESC_W))
		raise_exception_with_error(EX_GP, sel);
}

uint32 fastcall virtual_execute(uint16 sel, uint64 desc, uint32 va, size_t size)
{
	int32 pa;

	/* because only cs segment use this, no need to check S and T */
	if (!check_privilege_level(sel, desc))
		raise_exception(EX_GP);

	pa = virtual_access(desc, va, size);
	return memory_read(pa, size);
}

uint32 fastcall virtual_read(uint16 sel, uint64 desc, uint32 va, size_t size)
{
	int32 pa;

	/* data-segment always readable, code-segment need to check R bit */
	if (!(desc & DESC_T) && !(desc & DESC_R))
		raise_exception(EX_GP);

	if (!check_privilege_level(sel, desc))
		raise_exception(EX_GP);

	pa = virtual_access(desc, va, size);
	return memory_read(pa, size);
}

void fastcall virtual_write(uint16 sel, uint64 desc, uint32 va, uint32 data, size_t size)
{
	int32 pa;

	/* code-segment never writable, data-segment need to check W bit */
	if ((desc & DESC_T) || !(desc & DESC_W))
		raise_exception(EX_GP);

	if (!check_privilege_level(sel, desc))
		raise_exception(EX_GP);

	pa = virtual_access(desc, va, size);
	memory_write(pa, data, size);
}

uint32 fastcall paging(uint32 la)
{
	uint32 pdi, pde, pa;

	if (!(cr0 & CR0_PG))
		return la;

	pdi = (cr3 & CR3_PDB) | (pgdir(la) << 2);
	pde = memory_read(pdi, 4);
	if (!(pde & PDE_P))
		raise_exception(EX_PF);

	if (!(pde & PDE_PS))
		pa = paging_4kb(la, pde);
	else if (cr4 & CR4_PAE)
		pa = paging_2mb(la, pde);
	else if (cr4 & CR4_PSE)
		pa = paging_4mb(la, pde);
	else
		pa = paging_4kb(la, pde);

	// TODO: paging check & dirty
	return pa;
}

/* ------------------------------------------------------------------------- */

static inline uint32 seg_base(uint64 desc)
{
	uint32 base;

	base = ((uint32)(desc) & 0xffff0000) >> 16;
	base |= (uint32)((desc & 0xff00000000000000) >> 32);
	base |= (uint32)((desc & 0x000000ff00000000) >> 16);
	return base;
}

static inline uint32 seg_limit(uint64 desc)
{
	uint32 limit;

	limit = (uint32)(desc) & 0x0000ffff;
	limit |= (uint32)((desc & 0x000f000000000000) >> 32);
	return limit;
}

static inline bool check_privilege_level(uint16 sel, uint64 desc)
{
	int cpl = sel_cpl(sel);
	int dpl = desc_dpl(desc);

	/* code-segment check */
	if ((desc & DESC_S) && !(desc & DESC_T)) {
		if (desc & DESC_C)
			return cpl >= dpl;
		else
			return cpl == dpl;
	}

	return cpl <= dpl;
}

static inline uint32 paging_4kb(uint32 la, uint32 pde)
{
	uint32 pti, pte;

	pti = (pde & PDE_PTB) | (pgtab(la) << 2);
	pte = memory_read(pti, 4);
	if (!(pte & PTE_P))
		raise_exception(EX_PF);

	return (pte & PTE_PB) | pgoff_4kb(la);
}

static inline uint32 paging_4mb(uint32 la, uint32 pde)
{
	return (pde & PDE_PTB) | pgoff_4mb(la);
}

static inline uint32 paging_2mb(uint32 la, uint32 pde)
{
	assert(0);
	return la;
}

static uint32 fastcall virtual_access(uint64 desc, uint32 va, size_t size)
{
	uint32 la, pa, limit;

	assert(size);

	/* real mode? */
	if (!(cr0 & CR0_PE))
		return seg_base(desc) + (va & 0xffff);

	/* limit check */
	limit = seg_limit(desc);
	if (desc & DESC_G)
		limit = (limit << 12) | 0x00000fff;

	/* expand-down segment? */
	if ((desc & DESC_T) && (desc & DESC_E)) {
		if (va <= limit)
			raise_exception(EX_GP);
	} else {
		if (va + size > limit)
			raise_exception(EX_GP);
	}

	/* To enable paging, both the PE and PG flags must be set */
	la = seg_base(desc) + va;

	pa = paging(la);

	return pa;
}

/* ------------------------------------------------------------------------- */
